/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.transas.zorlog.gcslink;

import com.transas.zorlog.data.DataSource;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;
import java.util.EnumMap;
import java.util.Map;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 *
 * @author amalikov
 */
public class GCSLink implements Runnable {

	public GCSLink(
			SerialStream port,
			String state, String roll, String pitch, String heading,
			String airspeed, String baroalt, String vspeed, 
			String course, String gspeed, String latitude, String longitude, 
			String altitude, String rpm, String temp1, String temp2,
			String aileron, String elevator, String rudder, String throttle, String flaps,
			String failures, String voltage, String current, String fuel) {
		this.port = port;
		values = new EnumMap<>(ValuesEnum.class);
		values.put(ValuesEnum.STATE, new Value(state));
		values.put(ValuesEnum.ROLL, new Value(roll));
		values.put(ValuesEnum.PITCH, new Value(pitch));
		values.put(ValuesEnum.HEADING, new Value(heading));
		values.put(ValuesEnum.AIRSPEED, new Value(airspeed));
		values.put(ValuesEnum.BAROALT, new Value(baroalt));
		values.put(ValuesEnum.VSPEED, new Value(vspeed));
		values.put(ValuesEnum.COURSE, new Value(course));
		values.put(ValuesEnum.GSPEED, new Value(gspeed));
		values.put(ValuesEnum.LATITUDE, new Value(latitude));
		values.put(ValuesEnum.LONGITUDE, new Value(longitude));
		values.put(ValuesEnum.ALTITUDE, new Value(altitude));
		values.put(ValuesEnum.RPM, new Value(rpm));
		values.put(ValuesEnum.TEMP1, new Value(temp1));
		values.put(ValuesEnum.TEMP2, new Value(temp2));
		values.put(ValuesEnum.AILERON, new Value(aileron));
		values.put(ValuesEnum.ELEVATOR, new Value(elevator));
		values.put(ValuesEnum.RUDDER, new Value(rudder));
		values.put(ValuesEnum.THROTTLE, new Value(throttle));
		values.put(ValuesEnum.FLAPS, new Value(flaps));
		values.put(ValuesEnum.FAILURES, new Value(failures));
		values.put(ValuesEnum.VOLTAGE, new Value(voltage));
		values.put(ValuesEnum.CURRENT, new Value(current));
		values.put(ValuesEnum.FUEL, new Value(fuel));
		stop = false;
	}
	
	@Override
	public void run() {
		while (!stop) {
			try {
				ByteBuffer packet = getPacket();
				if (packet != null)
					processPacket(packet);
				else
					port.clean();
			} catch (IOException ex) {
				Logger.getLogger(GCSLink.class.getName()).log(Level.SEVERE, null, ex);
			}
		}
		port.close();
	}

	public void close() {
		stop = true;
	}
	
	private ByteBuffer getPacket() throws IOException {
		byte[] t = new byte[4];
		
		if (port.read(t, 0, t.length) == -1)
			return null;
		
		int size = ByteBuffer.wrap(t).order(ByteOrder.LITTLE_ENDIAN).get(2) & 0xFF;
			
		if (size < 4)
			return null;
			
		byte[] buf = Arrays.copyOf(t, size);
	
		if (port.read(buf, 4, size - 4) == -1)
			return null;
		
		if (getCrc(buf, size - 1) != ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN).get(size - 1))
			return null;
		
		return ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
	}
	
	
	public synchronized void setData(DataSource ds, int index) {
		for (Value v : values.values())
			v.value = ds.getValue(ds.getHandle(v.name), index);
	}
	
	private void processPacket(ByteBuffer packet) {
		ByteBuffer result = null;
		
		switch (packet.get(1)) {
		case 0x00:
			result = getVersion();
			break;
		case 0x10:
			result = getTelemetry();
			break;
		case 0x12:
			result = getTelemetry2();
			break;
		case 0x14:
			result = getTelemetry3();
			break;
		case 0x20:
			result = getManualTelemetry();
			break;
		case 0x22: case 0x30: case 0x40: case (byte)0x80:
			result = getEmpty(packet.get(1));
			break;
		default:
			return;
		}
		
		byte[] resarray = result.array();
		port.write(resarray, 0, resarray.length);
	}

	private ByteBuffer getVersion() {
		byte[] buf = new byte[21];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)0x01);
		result.put((byte)buf.length);
		result.put((byte)3);
		result.put(new byte[16]);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}
	
	private synchronized ByteBuffer getTelemetry() {
		byte[] buf = new byte[49];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)0x11);
		result.put((byte)buf.length);
		
		/*byte state = (byte)values.get(ValuesEnum.STATE).value;
		if (state == (byte)0xC1)
			state = (byte)0x61;
		result.put(state);
		*/
		result.put((byte)values.get(ValuesEnum.STATE).value);		
		result.putShort((short)(values.get(ValuesEnum.ROLL).value * 100));
		result.putShort((short)(values.get(ValuesEnum.PITCH).value * 100));
		result.putShort((short)(values.get(ValuesEnum.HEADING).value * 100));
		result.putShort((short)(values.get(ValuesEnum.AIRSPEED).value / 3.6 * 100));
		result.putShort((short)values.get(ValuesEnum.BAROALT).value);
		result.putShort((short)(values.get(ValuesEnum.VSPEED).value * 100));
		result.putShort((short)0);
		result.putShort((short)(values.get(ValuesEnum.COURSE).value * 100));
		result.putShort((short)(values.get(ValuesEnum.GSPEED).value / 3.6 * 100));
		result.putFloat((float)values.get(ValuesEnum.LATITUDE).value);
		result.putFloat((float)values.get(ValuesEnum.LONGITUDE).value);
		result.putShort((short)values.get(ValuesEnum.ALTITUDE).value);
		result.putShort((short)values.get(ValuesEnum.RPM).value);
		result.putShort((short)values.get(ValuesEnum.TEMP1).value);
		result.putShort((short)values.get(ValuesEnum.TEMP2).value);
		result.put((byte)(values.get(ValuesEnum.AILERON).value * 127));
		result.put((byte)(values.get(ValuesEnum.ELEVATOR).value * 127));
		result.put((byte)(values.get(ValuesEnum.RUDDER).value * 127));
		result.put((byte)(values.get(ValuesEnum.THROTTLE).value * 127));
		result.put((byte)values.get(ValuesEnum.FLAPS).value);
		result.put((byte)0);
		result.putShort((short)0);
		result.putShort((short)0);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}

	private synchronized ByteBuffer getTelemetry2() {
		byte[] buf = new byte[13];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)0x13);
		result.put((byte)buf.length);
		result.putInt((int)values.get(ValuesEnum.FAILURES).value);
		result.putShort((short)(values.get(ValuesEnum.VOLTAGE).value * 100));
		result.putShort((short)(values.get(ValuesEnum.CURRENT).value * 100));
		result.put((byte)values.get(ValuesEnum.FUEL).value);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}

	private ByteBuffer getTelemetry3() {
		byte[] buf = new byte[18];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)0x15);
		result.put((byte)buf.length);
		result.put(new byte[14]);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}
	
	private synchronized ByteBuffer getManualTelemetry() {
		byte[] buf = new byte[18];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)0x21);
		result.put((byte)buf.length);
		result.putShort((short)(values.get(ValuesEnum.ROLL).value * 100 + 10));
		result.putShort((short)(values.get(ValuesEnum.PITCH).value * 100 + 10));
		result.putShort((short)(values.get(ValuesEnum.HEADING).value * 100));
		result.putShort((short)(values.get(ValuesEnum.AIRSPEED).value / 3.6 * 100));
		result.putShort((short)values.get(ValuesEnum.BAROALT).value);
		result.putShort((short)(values.get(ValuesEnum.VSPEED).value * 100));
		result.putShort((short)0);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}

	private ByteBuffer getEmpty(byte id) {
		byte[] buf = new byte[4];
		ByteBuffer result = ByteBuffer.wrap(buf).order(ByteOrder.LITTLE_ENDIAN);
		result.put((byte)0);
		result.put((byte)(id + 1));
		result.put((byte)buf.length);
		result.put(getCrc(buf, buf.length - 1));
		return result;
	}
	
	
	
	/**
	 * Returns the CRC8 value for the given byte array
	 * @param data - array of bytes
	 * @param size - number of bytes
	 */
	private byte getCrc(final byte[] data, int size) {
		byte crc = (byte)0xFF;
		
		for (int i = 0; i < size; ++i) {
			crc ^= data[i];
			
			for (int j = 0; j < 8; ++j)
				crc = (byte)(((crc & 0x80) != 0) ? (crc << 1) ^ 0x31 : (crc << 1));
		}
		
		return crc;
	}
	
	
	private boolean stop;
	private SerialStream port;
	private Map<ValuesEnum, Value> values;
	
	private class Value {
		Value(String name) {
			this.name = name;
			value = 0;
		}
		String name;
		double value;
	}
	
	private enum ValuesEnum {
		STATE, ROLL, PITCH, HEADING, AIRSPEED, BAROALT, VSPEED, COURSE, GSPEED, LATITUDE, LONGITUDE, 
		ALTITUDE, RPM, TEMP1, TEMP2, AILERON, ELEVATOR, RUDDER, THROTTLE, FLAPS, FAILURES, VOLTAGE,
		CURRENT, FUEL
	}
}
